/*
 * Magnetometer.cpp
 *
 *  Created on: Jan 14, 2014
 *      Author: ppks
 */


#include "common.h"
#include "IMU_Common.h"
#include "i2c.h"
#include "FlightController_Common.h"
#include "EEPROM.h"

/******************************************************************************/
/* Macro Definition                                                           */
/******************************************************************************/
#if !defined(RECORD_MAG_ORIENTATION_DATA)
  #define RECORD_MAG_ORIENTATION_DATA(X, Y, Z)  {imuData.magADC[ROLL]  = X; imuData.magADC[PITCH]  = Y; imuData.magADC[YAW]  = Z;}
#endif

/******************************************************************************/
/* Local Variables Declaration                                                */
/******************************************************************************/
static float   magGain[3] = {1.0, 1.0, 1.0};  /* gain for each axis, populated at sensor Init */
static uint8_t magInit = 0;

/******************************************************************************/
/* Local Functions Declaration                                                */
/******************************************************************************/

/******************************************************************************/
/* Function Code                                                              */
/******************************************************************************/
/*******************************************************************************
* Function Name: Magnetometer_Init()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void Magnetometer_Init(void)
{
#if (MAGNETOMETER_TYPE == HMC5883)
    int32_t xyz_total[3] = {0,0,0};   /* 32 bit totals so they won't overflow. */
    bool initSuccessFlag = true;    /* Error indicator */

    delay(50);  /* Wait before start */
    CommI2C_WriteReg(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); /* Reg A DOR=0x010 + MS1,MS0 set to pos bias */

    /* Note that the  very first measurement after a gain change maintains the same gain as the previous setting.
     * The new gain setting is effective from the second measurement and on.
     */

    CommI2C_WriteReg(MAG_ADDRESS, HMC58X3_R_CONFB, 2 << 5);  /* Set the Gain */
    CommI2C_WriteReg(MAG_ADDRESS, HMC58X3_R_MODE, 1);
    delay(100); /* wait 100ms */

    Magnetometer_ReadRawADC();  /* Get one sample, and discard it */

    for (uint8_t i = 0; i < 10; i++)
    {
        /* Collect 10 samples */
        CommI2C_WriteReg(MAG_ADDRESS, HMC58X3_R_MODE, 1);
        delay(100); /* wait 100ms */
        Magnetometer_ReadRawADC();   /* Get the raw values in case the scales have already been changed. */

        /* Since the measurements are noisy, they should be averaged rather than taking the max. */
        xyz_total[0] += imuData.magADC[0];
        xyz_total[1] += imuData.magADC[1];
        xyz_total[2] += imuData.magADC[2];

        /* Detect saturation. */
        if (-(1<<12) >= min(imuData.magADC[0], min(imuData.magADC[1], imuData.magADC[2])))
        {
            initSuccessFlag = false;
            break;  /* Breaks out of the for loop.  No sense in continuing if we saturated. */
        }
    }

    /* Apply the negative bias. (Same gain) */
    CommI2C_WriteReg(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); /* Reg A DOR=0x010 + MS1,MS0 set to negative bias. */

    for (uint8_t i = 0; i < 10; i++)
    {
        CommI2C_WriteReg(MAG_ADDRESS,HMC58X3_R_MODE, 1);
        delay(100); /* wait 100ms */
        Magnetometer_ReadRawADC();  /* Get the raw values in case the scales have already been changed. */

        /* Since the measurements are noisy, they should be averaged. */
        xyz_total[0] -= imuData.magADC[0];
        xyz_total[1] -= imuData.magADC[1];
        xyz_total[2] -= imuData.magADC[2];

        /* Detect saturation. */
        if (-(1<<12) >= min(imuData.magADC[0], min(imuData.magADC[1], imuData.magADC[2])))
        {
            initSuccessFlag = false;
            break;  /* Breaks out of the for loop.  No sense in continuing if we saturated. */
        }
    }

    magGain[0] = fabs(820.0 * HMC58X3_X_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[0]);
    magGain[1] = fabs(820.0 * HMC58X3_Y_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[1]);
    magGain[2] = fabs(820.0 * HMC58X3_Z_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[2]);

    /* leave test mode */
    CommI2C_WriteReg(MAG_ADDRESS, HMC58X3_R_CONFA, 0x70 ); /* Configuration Register A  -- 0 11 100 00  num samples: 8 ; output rate: 15Hz ; normal measurement mode */
    CommI2C_WriteReg(MAG_ADDRESS, HMC58X3_R_CONFB, 0x20 ); /* Configuration Register B  -- 001 00000    configuration gain 1.3Ga */
    CommI2C_WriteReg(MAG_ADDRESS, HMC58X3_R_MODE,  0x00 ); /* Mode register             -- 000000 00    continuous Conversion Mode */

    delay(100);

    /* Set a flag that magnetometer is initialized */
    magInit = 1;

    if (initSuccessFlag == false)
    {
        /* Something went wrong so get a best guess */
        magGain[0] = 1.0;
        magGain[1] = 1.0;
        magGain[2] = 1.0;
    }
#endif /* (MAGNETOMETER_TYPE == HMC5883) */
}

/*******************************************************************************
* Function Name: Magnetometer_ReadRawADC()
********************************************************************************
* Summary:
*
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
void Magnetometer_ReadRawADC(void)
{
    uint8_t rawADC[6];

    /* Read the data from accelerometer */
#if (MAGNETOMETER_TYPE == HMC5883)
    CommI2C_ReadRegToBuffer(MAG_ADDRESS, MAG_DATA_REGISTER, rawADC, 6);
#if (MAGNETOMETER_TYPE == HMC5843)
    /* Record the data into the global structure */
    RECORD_MAG_ORIENTATION_DATA( ((rawADC[0]<<8) | rawADC[1])>>3 ,  /* X */
                                 ((rawADC[2]<<8) | rawADC[3])>>3 ,  /* Y */
                                 ((rawADC[4]<<8) | rawADC[5])>>3 ); /* Z */
#else /* (MAGNETOMETER_TYPE == HMC5843) */
    /* Record the data into the global structure */
    RECORD_MAG_ORIENTATION_DATA( ((rawADC[0]<<8) | rawADC[1])>>3 ,  /* X */
                                 ((rawADC[4]<<8) | rawADC[5])>>3 ,  /* Y */
                                 ((rawADC[2]<<8) | rawADC[3])>>3 ); /* Z */
#endif /* (MAGNETOMETER_TYPE == HMC5843) */
#endif /* (MAGNETOMETER_TYPE == HMC5883) */
}

/*******************************************************************************
* Function Name: Magnetometer_GetNewData()
********************************************************************************
* Summary:
*  Reads the magnetometer if it is time to do so and returns 1. If it is not
*  time yet it returns zero and global structure is not updated
*
* Parameters:
*  None
*
* Return:
*  None
*
*******************************************************************************/
uint8_t Magnetometer_GetNewData(void)
{
    static uint32_t magDataReadTimestamp;
    static uint32_t tCal = 0;
    static int16_t magZeroTempMin[3];
    static int16_t magZeroTempMax[3];
    uint8_t axis;

    if (currentLoopTimeStamp < magDataReadTimestamp)
    {
        /* It is not time yet to read data. Each read is spaced by 100ms */
        return 0;
    }

    /* Update the timestamp for next read */
    magDataReadTimestamp = currentLoopTimeStamp + MAG_UPDATE_PERIOD;

    /* Read data from ADC */
    Magnetometer_ReadRawADC();

    /* Apply gains */
    imuData.magADC[ROLL]  = imuData.magADC[ROLL]  * magGain[ROLL];
    imuData.magADC[PITCH] = imuData.magADC[PITCH] * magGain[PITCH];
    imuData.magADC[YAW]   = imuData.magADC[YAW]   * magGain[YAW];

    if (IsMagCalibrated() == 0)
    {
        tCal = magDataReadTimestamp;

        /* Initialize data for calibration */
        for(axis=0; axis < 3; axis++)
        {
            calData.magBaseline[axis] = 0;
            magZeroTempMin[axis] = imuData.magADC[axis];
            magZeroTempMax[axis] = imuData.magADC[axis];
        }

        /* Clear the calibration request flag */
        SetMagCalibratedFlag();
    }

    if (magInit)
    {
        /* we apply offset only once mag calibration is done */
        imuData.magADC[ROLL]  -= calData.magBaseline[ROLL];
        imuData.magADC[PITCH] -= calData.magBaseline[PITCH];
        imuData.magADC[YAW]   -= calData.magBaseline[YAW];
    }

    if (tCal != 0)
    {
        if ((magDataReadTimestamp - tCal) < MAG_CALIBRATION_TIME)
        {
            /* 30s: you have 30s to turn the multi in all directions */
            LEDPIN_TOGGLE;

            for(axis = 0; axis < 3; axis++)
            {
                if (imuData.magADC[axis] < magZeroTempMin[axis])
                {
                    magZeroTempMin[axis] = imuData.magADC[axis];
                }

                if (imuData.magADC[axis] > magZeroTempMax[axis])
                {
                    magZeroTempMax[axis] = imuData.magADC[axis];
                }
            }
        }
        else
        {
            /* Calibration is done - reset timestamp */
            tCal = 0;

            /* Update baseline data in the structure */
            for (axis = 0; axis < 3; axis++)
            {
                calData.magBaseline[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) >> 1;
            }
        #if UPDATE_CAL_DATA_AFTER_ANY_SENSOR
            /* Update calibration data in EEPROM */
            EEPROM_UpdateCalibrationData();
        #endif /* UPDATE_CAL_DATA_AFTER_ANY_SENSOR */
        }
    }
    else
    {
        /* No calibration ongoing - process normal routine */
    #if defined(SENSORS_TILT_45DEG_LEFT)
        int16_t temp = ((imuData.magADC[PITCH] - imuData.magADC[ROLL] ) * 7) / 10;
        imuData.magADC[ROLL] = ((imu.magADC[ROLL]  + imuData.magADC[PITCH]) * 7) / 10;
        imuData.magADC[PITCH] = temp;
    #endif
    #if defined(SENSORS_TILT_45DEG_RIGHT)
        int16_t temp = ((imuData.magADC[PITCH] + imuData.magADC[ROLL] ) * 7) / 10;
        imuData.magADC[ROLL] = ((imuData.magADC[ROLL]  - imuData.magADC[PITCH]) * 7) / 10;
        imuData.magADC[PITCH] = temp;
    #endif
    }

    /* Return a flag that new data is available */
    return 1;
}
